#include "ErrorState.hpp"
#include "Event.hpp"
#include "EventType.hpp"
#include "FreestyleState.hpp"
#include "Robot.hpp"

#include "HardWare.hpp"

#define MSINREVERSE 1000
#define MSTURNINGRIGHT 500

FreestyleState::FreestyleState() :
	currentStatus(FORWARD),
	timeLeft(0)
{

}

void FreestyleState::handleEvent(Event& event)
{
	switch (event.getEventType())
	{
		case COLLISION: //Collision occurred
		case PROXIMITY: //Low proximity
		{
//			if (currentStatus == FORWARD) //obstacle in front
//			{
				avoidObstacle();
//			}
//			else
//			{
//				//obstacle whilst in reverse (or whilst turning)
//				ErrorState state;
//				Robot::getInstance().changeState(state);
//			}
			break;
		}
		default: //Ignore everything else
		{
			break;
		}
	}
}

void FreestyleState::startState()
{
//	//If not driving forward -> resume current state
//	if (currentStatus != FORWARD)
//		resumeAvoidObstacle(currentStatus);
//	else //Else drive forward
		Robot::getInstance().getEngine().driveForward();
}

void FreestyleState::avoidObstacle()
{
	//start driving backward
	currentStatus = BACKWARD;
	Robot::getInstance().getEngine().driveBackward();
	//wait 1 sec
	timeLeft = MSINREVERSE;
	Hardware::getHardwareController().wait(&timeLeft);
	//start turning right
	currentStatus = TURNING;
	Robot::getInstance().getEngine().turnRight();
	timeLeft = MSTURNINGRIGHT;
	//wait 250ms
	Hardware::getHardwareController().wait(&timeLeft);
	//drive forward
	currentStatus = FORWARD;
	Robot::getInstance().getEngine().driveForward();
}

//void FreestyleState::partialWait()
//{
//	unsigned int ms = 50;
//	if (ms > timeLeft)
//		ms = timeLeft;
//	timeLeft -= ms;
//	HardwareController::getHardwareController().wait(ms);
//}
//
//void FreestyleState::resumeAvoidObstacle(Status status)
//{
//	switch (status)
//	{
//		case BACKWARD:
//		{
//			Robot::getInstance().getEngine().driveBackward();
//			HardwareController::getHardwareController().wait(&timeLeft);
//			timeLeft = MSTURNINGRIGHT;
//			//No break
//		}
//		case TURNING:
//		{
//			currentStatus = TURNING;
//			Robot::getInstance().getEngine().turnRight();
//			HardwareController::getHardwareController().wait(&timeLeft);
//			//No break
//		}
//		default:
//		{
//			currentStatus = FORWARD;
//			Robot::getInstance().getEngine().driveForward();
//			break;
//		}
//	}
//}

State::StateType FreestyleState::getStateType()
{
	return State::FREESTYLE;
}
